#include <iostream>

using namespace std;

// 定义卡尔曼滤波参数
float Q = 0.1; // 系统过程噪声协方差  如果这个值设置得很小，那么滤波器将会更加相信模型的预测；而如果设置得很大，滤波器就会更加相信观测到的数据。
float R = 0.8; // 观测噪声协方差      如果这个值设置得很小，滤波器将会更加相信传感器的测量值；而如果设置得很大，滤波器就会更加相信模型的预测。
float X = 1;   // 状态估计            最终结果
float P = 1;   // 状态估计协方差      如果这个值设置得很小，滤波器将会更加相信状态估计值；而如果设置得很大，滤波器就会更加相信观测到的数据。

// 卡尔曼滤波函数
float kalmanFilter(float Z)
{
    // 预测
    float X_pred = X;
    float P_pred = P + Q;
    // 更新
    float K = P_pred / (P_pred + R);
    X = X_pred + K * (Z - X_pred);
    P = (1 - K) * P_pred;
    return X;
}

// pre_v = a*v + b*u
// p = a*a*p*p+q
// g = p*h/(p*h*h+r)
// v = pre_v +(new_v - pre_v)*g
// p = (1-g*h)*p
int main()
{
    // 给定的组数据
    float num_arr[] = {1, 1.05, 1.1, 1, 2, 1.1, 0.89, 1.2, 1, 0.77, 1, 0.77};
    int n = sizeof(num_arr) / sizeof(float);
    // 对数据进行滤波并输出结果
    for (int i = 0; i < n; ++i)
    {
        float filtered_value = kalmanFilter(num_arr[i]);
        cout << "Filtered value at time " << i + 1 << " " << num_arr[i] << ": " << filtered_value << endl;
    }
    return 0;
}
